#include "ModbusService.hpp"
#include "RTUSlave.hpp"
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "ch32v30x_libs.h"
#include "RTUFunctions.hpp"

using namespace Modbus;

SemaphoreHandle_t mutex_modbus = nullptr;
TaskHandle_t task_modbus = nullptr;
QueueHandle_t queue_modbus_rx = nullptr;

class RTUPortable : public RTUSlave::PortableStrategy {
public:
    virtual bool init(){
        return true;
    }

    virtual bool deinit(){
        return true;
    }

    virtual bool lock(){
        if ( mutex_modbus != nullptr ){
            xSemaphoreTakeRecursive(mutex_modbus,portMAX_DELAY);
        }
        return true;
    }

    virtual bool unlock(){
        if ( mutex_modbus != nullptr ){
            xSemaphoreGiveRecursive(mutex_modbus);
        }
        return true;
    }

    virtual int send(void *frame,unsigned int size){
        for ( int n = 0; n < size; n++ ){
            USART_SendData(USART2,reinterpret_cast<char*>(frame)[n]);
            while ( USART_GetFlagStatus(USART2,USART_FLAG_TXE) == RESET );
        }
        return RTUSlave::Result::SUCCESS;
    }

};

class ReadCoilsPortable : public Function::ReadCoils::PortableStrategy {
public:
    virtual bool getBit(uint8_t *buffer,unsigned int address){
        if ( address % 2 == 0 ){
            *buffer = 0;
        }else{
            *buffer = 1;
        }
        return true;
    }
};

class ReadRegisterPortable : public Function::ReadHoldingRegister::PortableStrategy {
public:
    virtual bool getWord(uint16_t *buffer,unsigned int address){
        *buffer = static_cast<uint16_t>(address + base_count++);
        return true;
    }
private:
    unsigned int base_count = 0;
};

RTUPortable rtu_portable;
ReadCoilsPortable read_coils_portable;
ReadRegisterPortable read_register_portable;
RTUSlave *rtu_slave;
bool cut_flag = false;

void task(void *param){
    uint8_t byte = 0x00;
    while ( true ){
        if ( rtu_slave == nullptr ||  queue_modbus_rx == nullptr ){
            vTaskDelay(1000);
        }
        if ( xQueueReceive(queue_modbus_rx,&byte,400) != pdPASS ){
            rtu_slave->cut();
        }else{
            rtu_slave->feed(byte);
        }
    }
}



void board_init(void){
    GPIO_InitTypeDef gpio_init_structure = {0};
    USART_InitTypeDef usart_init_structure = {0};
    NVIC_InitTypeDef nvic_init_structure = {0};

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);

    gpio_init_structure.GPIO_Pin = GPIO_Pin_2;
    gpio_init_structure.GPIO_Mode = GPIO_Mode_AF_PP;
    gpio_init_structure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA,&gpio_init_structure);
    gpio_init_structure.GPIO_Pin = GPIO_Pin_3;
    gpio_init_structure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_Init(GPIOA,&gpio_init_structure);

    usart_init_structure.USART_BaudRate = 460800;
    usart_init_structure.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
    usart_init_structure.USART_Parity = USART_Parity_No;
    usart_init_structure.USART_WordLength = USART_WordLength_8b;
    usart_init_structure.USART_StopBits = USART_StopBits_1;
    usart_init_structure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_Init(USART2,&usart_init_structure);
    
    nvic_init_structure.NVIC_IRQChannel = USART2_IRQn;
    nvic_init_structure.NVIC_IRQChannelPreemptionPriority = 1;
    nvic_init_structure.NVIC_IRQChannelSubPriority = 0;
    nvic_init_structure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&nvic_init_structure);

    USART_ITConfig(USART2,USART_IT_RXNE,ENABLE);
    // USART_ITConfig(USART2,USART_IT_IDLE,ENABLE);
    USART_Cmd(USART2,ENABLE);
}

uint8_t tx_buffer[1024];
uint8_t rx_buffer[1024];
uint16_t read_register_func_buffer[256];
uint16_t read_coils_func_buffer[256];
Function::ReadHoldingRegister *func_read_holding_register;
Function::ReadCoils *func_read_coils;

bool ModbusService::run(){
    board_init();
    RTUSlave::Configuration conf = {
        .address = 0x01,
        .tx_buffer = tx_buffer,
        .tx_buffer_size = 1024,
        .rx_buffer = rx_buffer,
        .rx_buffer_size = 1024,
    };

    mutex_modbus = xSemaphoreCreateRecursiveMutex();
    queue_modbus_rx = xQueueCreate(1024,sizeof(char));

    rtu_slave = new RTUSlave(&rtu_portable,conf);

    func_read_coils = new Function::ReadCoils(read_coils_func_buffer,256,&read_coils_portable,rtu_slave);
    func_read_holding_register = new Function::ReadHoldingRegister(read_register_func_buffer,256,&read_register_portable,rtu_slave);

    rtu_slave->addFunction(func_read_coils);
    rtu_slave->addFunction(func_read_holding_register);

    xTaskCreate(task,"RTU",2048,nullptr,5,&task_modbus);
    
    return true;
}

extern "C" __attribute__((interrupt()))
void USART2_IRQHandler(void){
    if ( USART_GetFlagStatus(USART2,USART_FLAG_RXNE) != RESET ){
        uint8_t byte = USART_ReceiveData(USART2);
        USART_ClearFlag(USART2,USART_FLAG_RXNE);
        BaseType_t temp;
        xQueueSendFromISR(queue_modbus_rx,&byte,&temp);
    }
}